/**
 * \file
 *
 * \brief Empty user application template
 *
 */

/*
 * Include header files for all drivers that have been imported from
 * AVR Software Framework (ASF).
 */
#include <asf.h>
#include <avr/pgmspace.h>

#define WRITE		0x00
#define READ		0x01
#define NUNCHUCK	0xA4

void usart_send_str_P(USART_t * MyUSART, const char * str);
void usart_send_hex(USART_t * MyUSART, uint8_t value);
uint8_t twi_send_msg(TWI_t * MyTWI, uint8_t address, uint8_t * data, uint16_t msg_length);
uint8_t twi_read_msg(TWI_t * MyTWI, uint8_t address, uint8_t register_address, uint8_t * data, uint16_t msg_length);

int main (void)
{
	// Initialize LEDs
	PORTE.DIR = 0xFF;
	PORTE.OUT = 0xFF;

	// Change clock source -> 32MHz internal RC
	OSC.CTRL = OSC_RC32MEN_bm;
	while((OSC.STATUS & OSC_RC32MRDY_bm) == 0) // Wait for the 32MHz internal RC to be ready
	{
		PORTE.OUTTGL = PIN4_bm;
	}
	CCP = CCP_IOREG_gc; // Secure clock select procedure
	CLK.CTRL = CLK_SCLKSEL_RC32M_gc;

	// Initialize SWITCHes
	PORTF.DIR = 0x00;
	PORTCFG.MPCMASK = 0xFF;
	PORTF.PIN0CTRL = PORT_OPC_PULLUP_gc;
	
	// Initialize TCE0
	TCE0.PER = 0x007F;
	TCE0.CCA = 0x0080;
	TCE0.CCB = 0x0080;
	TCE0.CCC = 0x0080;
	TCE0.CCD = 0x0080;
	TCE0.CTRLB = 0xF0 | TC_WGMODE_SS_gc; // Enable CCD, CCC, CCB and CCA in single slope PWM
	TCE0.CTRLA = TC_CLKSEL_DIV1_gc;
	
	// Initialize USARTC0
	PORTC.DIRSET = PIN3_bm; // Tx as output
	PORTC.DIRCLR = PIN2_bm; // Rx as input
	USARTC0.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc;
	USARTC0.BAUDCTRLA = 12; // 9600bps @ 32MHz
	USARTC0.BAUDCTRLB = (4 << USART_BSCALE_gp); // Better accuracy with fractionnal baud rate generator
	USARTC0.CTRLB = USART_RXEN_bm | USART_TXEN_bm;
	
	// Initialize TWID
	TWID.MASTER.BAUD = 35; // 400 kHz @ 32 MHz
	TWID.MASTER.CTRLA = TWI_MASTER_ENABLE_bm;
	TWID.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; // Force bus state to idle
	
	while(PORTF.IN & PIN0_bm)
	{
		for (uint8_t pattern = 0x01; pattern > 0; pattern *= 2)
		{
			PORTE.OUTCLR = pattern;
			volatile uint16_t delay = 0xFFFF;
			while (delay-- > 0) ;
			PORTE.OUTSET = pattern;
		}
		
		for (uint8_t pattern = 0x80; pattern > 0; pattern /= 2)
		{
			PORTE.OUTCLR = pattern;
			volatile uint16_t delay = 0xFFFF;
			while (delay-- > 0) ;
			PORTE.OUTSET = pattern;
		}
	}
	
	uint8_t MyData[6];
	
	//usart_send_str_P(&USARTC0, PSTR("Initialisation du Nunchuck "));
	
	MyData[0] = 0xF0;
	MyData[1] = 0x55;
	if(0 != twi_send_msg(&TWID, NUNCHUCK, &MyData[0], 2))
	{
		usart_send_str_P(&USARTC0, PSTR("Initialisation du Nunchuck KO - 1\r\n"));
	}
	else
	{
		volatile uint16_t delay = 0xFFFF;
		while (delay-- > 0) ;
		
		MyData[0] = 0xFB;
		MyData[1] = 0x00;
		if(0 != twi_send_msg(&TWID, NUNCHUCK, &MyData[0], 2))
		{
			usart_send_str_P(&USARTC0, PSTR("Initialisation du Nunchuck KO - 2\r\n"));
		}
		else
		{
			//usart_send_str_P(&USARTC0, PSTR("OK\r\n"));
		}
	}

	while(1)
	{
		volatile uint16_t delay = 0xFFFF;
		while (delay-- > 0) ;

		// Read Nunchuck
		if(0 == twi_read_msg(&TWID, NUNCHUCK, 0x00, &MyData[0], 6))
		{
			// Start of sequence
			usart_send_str_P(&USARTC0, PSTR("\r\n"));
			// Joystick X
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[0];
			// Joystick Y
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[1];
			// Accelerometer X [9:2]
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[2];
			// Accelerometer Y [9:2]
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[3];
			// Accelerometer Z [9:2]
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[4];
			// Az[1:0] | Ay[1:0] | Ax[1:0] | Bc | Bz
			while((USARTC0.STATUS & USART_DREIF_bm) == 0) ;
			USARTC0.DATA = MyData[5];
			
			if (MyData[0] & 0x80) // MyData[0] is negative
			{
				// 2's complement
				uint8_t MyTemp = ~MyData[0];
				MyTemp = MyTemp + 1;
				
				TCE0.CCA = MyTemp;
				TCE0.CCB = 0x0080;
			}
			else
			{
				TCE0.CCA = 0x0080;
				TCE0.CCB = MyData[0];
			}
			
			if (MyData[1] & 0x80) // MyData[1] is negative
			{
				// 2's complement
				uint8_t MyTemp = ~MyData[1];
				MyTemp = MyTemp + 1;
				
				TCE0.CCC = MyTemp;
				TCE0.CCD = 0x0080;
			}
			else
			{
				TCE0.CCC = 0x0080;
				TCE0.CCD = MyData[1];
			}

			PORTE.OUTSET = (MyData[5] & 0x03) << 4;
			PORTE.OUTCLR = (~MyData[5] & 0x03) << 4;
		}
		else
			usart_send_str_P(&USARTC0, PSTR("Erreur I2C\r\n"));
	}
}

void usart_send_str_P(USART_t * MyUSART, const char * str)
{
	char c;
	
	while((c = pgm_read_byte(str++)))
	{
		while((MyUSART->STATUS & USART_DREIF_bm) == 0) ;
		MyUSART->DATA = c;
	}
}

void usart_send_hex(USART_t * MyUSART, uint8_t value)
{
	uint8_t u_nib, l_nib;

	u_nib = 0xF0 & value;	// keep only upper nibble
	u_nib = u_nib >> 4;	// and shift to lower nibble
	l_nib = 0x0F & value;	// keep only lower nibble

	u_nib += 0x30;		// add offset to ASCII '0'
	if (u_nib > '9')
		u_nib += 7; 	// if alphabetic add offset to ASCII 'A'

	l_nib += 0x30;		// add offset to ASCII '0'
	if (l_nib > '9')
		l_nib += 7; 	// if alphabetic add offset to ASCII 'A'
	
	while((MyUSART->STATUS & USART_DREIF_bm) == 0) ;
	MyUSART->DATA = u_nib;

	while((MyUSART->STATUS & USART_DREIF_bm) == 0) ;
	MyUSART->DATA = l_nib;
}

uint8_t twi_send_msg(TWI_t * MyTWI, uint8_t address, uint8_t * data, uint16_t msg_length)
{
	// Check bus state before doing anything, not idle -> exit with code 1
	if((MyTWI->MASTER.STATUS & TWI_MASTER_BUSSTATE_gm) != TWI_MASTER_BUSSTATE_IDLE_gc)
		return 1;

	// Send slave address
	MyTWI->MASTER.ADDR = address | WRITE;
	while((MyTWI->MASTER.STATUS & TWI_MASTER_WIF_bm) == 0) ;	// loop until transaction is complete
	if(MyTWI->MASTER.STATUS & TWI_MASTER_RXACK_bm) // No ACK from slave -> STOP ans exit with code 2
	{
		MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
		return 2;
	}
	
	// Send data
	for(uint16_t i = 0; i < msg_length; i++)
	{
		MyTWI->MASTER.DATA = *(data + i);
		while((MyTWI->MASTER.STATUS & TWI_MASTER_WIF_bm) == 0) ;	// loop until transaction is complete
		if(MyTWI->MASTER.STATUS & TWI_MASTER_RXACK_bm) // No ACK from slave -> STOP ans exit with code 2
		{
			MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
			return (3 + i);
		}
	}
	
	// Send stop and exit with code 0 (no error)
	MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
	return 0;
}

uint8_t twi_read_msg(TWI_t * MyTWI, uint8_t address, uint8_t register_address, uint8_t * data, uint16_t msg_length)
{
	// Check bus state before doing anything, not idle -> exit with code 1
	if((MyTWI->MASTER.STATUS & TWI_MASTER_BUSSTATE_gm) != TWI_MASTER_BUSSTATE_IDLE_gc)
		return 1;

	// Send slave address
	MyTWI->MASTER.ADDR = address | WRITE;
	while((MyTWI->MASTER.STATUS & TWI_MASTER_WIF_bm) == 0) ;	// loop until transaction is complete
	if(MyTWI->MASTER.STATUS & TWI_MASTER_RXACK_bm) // No ACK from slave -> STOP ans exit with code 2
	{
		MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
		return 2;
	}
	
	// Send register_address
	MyTWI->MASTER.DATA = register_address;
	while((MyTWI->MASTER.STATUS & TWI_MASTER_WIF_bm) == 0) ;	// loop until transaction is complete
	if(MyTWI->MASTER.STATUS & TWI_MASTER_RXACK_bm) // No ACK from slave -> STOP ans exit with code 2
	{
		MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
		return 3;
	}

	// Restart
	MyTWI->MASTER.ADDR = address | READ;

	while((MyTWI->MASTER.STATUS & TWI_MASTER_RIF_bm) == 0){}	// loop until transaction is complete

	if(MyTWI->MASTER.STATUS & TWI_MASTER_RXACK_bm) // No ACK from slave -> STOP ans exit with code 2
	{
		MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; // release the bus with a stop condition
		return 4;
	}

	// Read data + send ACK
	for(uint16_t i = 0; i < (msg_length - 1); i++)
	{
		while((MyTWI->MASTER.STATUS & TWI_MASTER_RIF_bm) == 0){}	// loop until transaction is complete
		*(data + i) = MyTWI->MASTER.DATA;
		MyTWI->MASTER.CTRLC = TWI_MASTER_CMD_RECVTRANS_gc;
	}
	
	// Read last data + send NACK + STOP
	while((MyTWI->MASTER.STATUS & TWI_MASTER_RIF_bm) == 0){}	// loop until transaction is complete
	*(data + (msg_length - 1)) = MyTWI->MASTER.DATA;
	MyTWI->MASTER.CTRLC = TWI_MASTER_ACKACT_bm | TWI_MASTER_CMD_STOP_gc;
	
	// Exit with code 0 (no error)
	return 0;
}

/* bout de code qui scan le bus I2C pour trouver tous les peripheriques qui repondent present */
/*
usart_send_str_P(&USARTC0, PSTR("Recherche des peripheriques :\r\n"));

for(uint8_t temp_slave_address = 0x08; temp_slave_address > 0x06; temp_slave_address += 2)
{
	TWID.MASTER.ADDR = temp_slave_address;
	
	// loop until transaction is complete
	while((TWID.MASTER.STATUS & TWI_MASTER_WIF_bm) == 0) ;
	
	if((TWID.MASTER.STATUS & TWI_MASTER_RXACK_bm) == 0) // ACK received if RXACK is 0
	{
		usart_send_str_P(&USARTC0, PSTR("Trouve 0x"));
		usart_send_hex(&USARTC0, temp_slave_address);
		usart_send_str_P(&USARTC0, PSTR("\r\n"));
	}
	
	// release the bus with a stop condition
	TWID.MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc;
	
	// Delay
	uint16_t delay = 0x2FFF;
	while (delay-- > 0) ;
}
*/